#include "vtkVelodyneHDLPositionPacketInterpreter.h"
#include "vtkVelodyneHDLPositionPacket.h"

#include <vtkDoubleArray.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkPointData.h>
#include <vtkTransform.h>
#include <vtkTupleInterpolator.h>

#include "vtkCustomTransformInterpolator.h"
#include "NMEAParser.h"
#include "statistics.h"

#include <iomanip>
#include <sstream>

#include <boost/algorithm/string.hpp>

namespace  {
  template <typename T>
  void AddArrayToTable(T & array, vtkTable* table, char const * name)
  {
    array = T::New();
    array->SetName(name);
    table->AddColumn(array);
  }
}

//-----------------------------------------------------------------------------
// This function is not used anymore
// It was used to apply the transform to lidar points
// This mecanism is broken by thi commit :
// message commit : "Prepare multi sensor support to the Lidar Provider and clean up a bit"
// sha : f7b88847830c213e4df72fbbb660c3fcf80e80b4
void vtkVelodyneHDLPositionPacketInterpreter::FillInterpolatorFromGPSAndHeading(
  vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* headings)
{
  vtkNew<vtkTupleInterpolator> headingInterpolator;

  // assert(gpsTime is sorted)
  assert(points->GetNumberOfPoints() == times->GetNumberOfTuples());
  this->Interp->SetInterpolationTypeToLinear();
  this->Interp->Initialize();
  headingInterpolator->SetInterpolationTypeToLinear();
  headingInterpolator->SetNumberOfComponents(2);

  assert(times->GetNumberOfTuples() == gpsTime->GetNumberOfTuples());

  double lastGPS = 0.0;
  for (vtkIdType i = 0, k = times->GetNumberOfTuples(); i < k; ++i)
  {
    const double currGPS = gpsTime->GetTuple1(i);
    if (currGPS != lastGPS)
    {
      // Get position and heading
      double pos[3];
      points->GetPoint(i, pos);

      const double heading = headings->GetTuple1(i);

      // Check the input data
      bool isTranslationFinite =
        vtkMath::IsFinite(pos[0]) && vtkMath::IsFinite(pos[1]) && vtkMath::IsFinite(pos[2]);
      bool isRotationFinite = vtkMath::IsFinite(heading);

      // Compute transform
      // Here we want to compute the transform to go
      // from the solid referential frame to the world
      // georeferenced frame. Hence, given the position
      // and orientation of the GPS in the solid frame we
      // need to apply the transform from the solid frame
      // to the GPS (backward gps pose) and then the transform
      // from the GPS to the world georeferenced frame
      vtkNew<vtkTransform> transformGpsWorld, transformVehiculeWorld;
      transformGpsWorld->PostMultiply();
      if (isRotationFinite)
        transformGpsWorld->RotateZ(heading);
      else
        vtkGenericWarningMacro("Error in GPS rotation");
      if (isTranslationFinite)
        transformGpsWorld->Translate(pos);
      else
        vtkGenericWarningMacro("Error in GPS position");

      // Compute transform from vehicule to GPS
      // and then compose with the transform GPS to world
      vtkNew<vtkMatrix4x4> gpsToWorld, vehiculeToGps, vehiculeToWorld;
      this->SensorTransform->GetMatrix(vehiculeToGps.Get());
      transformGpsWorld->GetMatrix(gpsToWorld.Get());
      vehiculeToGps->Invert();
      vtkMatrix4x4::Multiply4x4(gpsToWorld.Get(), vehiculeToGps.Get(), vehiculeToWorld.Get());
      transformVehiculeWorld->SetMatrix(vehiculeToWorld.Get());
      transformVehiculeWorld->Modified();

      // Add the transform to the interpolator
      this->Interp->AddTransform(currGPS, transformVehiculeWorld.GetPointer());

      // Compute heading vector for interpolation
      double ha = heading * DEG_TO_RAD;
      double hv[2] = { cos(ha), sin(ha) };
      headingInterpolator->AddTuple(currGPS, hv);
    }
  }
}

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkVelodyneHDLPositionPacketInterpreter)

//-----------------------------------------------------------------------------
vtkVelodyneHDLPositionPacketInterpreter::vtkVelodyneHDLPositionPacketInterpreter()
{
  this->Interp = vtkSmartPointer<vtkCustomTransformInterpolator>::New();

  this->ResetCurrentData();

  this->pointcount = 0;
  this->Offset[0] = 0.0;
  this->Offset[1] = 0.0;
  this->Offset[2] = 0.0;

  this->UseGPGGASentences = false;
  this->PPSSynced = false;
  this->LastPPSState = PPSState::PPS_ABSENT;
  this->HasTimeshiftEstimation = false;
  this->TimeshiftMeasurements.clear();
  this->AssumedHardwareLag = 0.094; // always positive, in seconds

  this->hasLastGPSUpdateTime = false;
  this->lastGPSUpdateTime = 0.0;
  this->GPSTimeOffset = 0.0;
  this->convertedGPSUpdateTime = 0.0;

  this->hasLastLidarUpdateTime = false;
  this->lastLidarUpdateTime = 0.0;
  this->lidarTimeOffset = 0.0;

  this->previousConvertedGPSUpdateTime = -1.0; // negative means "no previous"
}

//----------------------------------------------------------------------------
void vtkVelodyneHDLPositionPacketInterpreter::InitArrays()
{
  this->points = vtkSmartPointer<vtkPoints>::New();

  AddArrayToTable(this->lats, this->RawInformation, "lat");
  AddArrayToTable(this->lons, this->RawInformation, "lon");
  AddArrayToTable(this->times, this->RawInformation, "time");
  AddArrayToTable(this->gpsTime, this->RawInformation, "gpsTime");

  AddArrayToTable(this->gyro1, this->RawInformation, "gyro1");
  AddArrayToTable(this->gyro2, this->RawInformation, "gyro2");
  AddArrayToTable(this->gyro3, this->RawInformation, "gyro3");
  AddArrayToTable(this->temp1, this->RawInformation, "temp1");
  AddArrayToTable(this->temp2, this->RawInformation, "temp2");
  AddArrayToTable(this->temp3, this->RawInformation, "temp3");
  AddArrayToTable(this->accel1x, this->RawInformation, "accel1x");
  AddArrayToTable(this->accel1y, this->RawInformation, "accel1y");
  AddArrayToTable(this->accel2x, this->RawInformation, "accel2x");
  AddArrayToTable(this->accel2y, this->RawInformation, "accel2y");
  AddArrayToTable(this->accel3x, this->RawInformation, "accel3x");
  AddArrayToTable(this->accel3y, this->RawInformation, "accel3y");
  AddArrayToTable(this->heading, this->RawInformation, "heading");

  this->PositionOrientation->SetPoints(this->points);

  // Add also Time array to the position orientation vtkPolyData:
  // So we can color by time
  this->PositionOrientation->GetPointData()->AddArray(this->times);
}


//----------------------------------------------------------------------------
bool vtkVelodyneHDLPositionPacketInterpreter::IsValidPacket(unsigned char const * packetData,
                                                  unsigned int dataLength)
{
  if (dataLength != 512)
  {
    // Data-Packet Specifications says that position-packets are 512 byte long.
    return false;
  }

  for (int i = 0; i < 14; ++i)
  {
    if (packetData[i] != 0)
    {
      std::cerr << "unexpected data in first zeros block\n";
      return false;
    }
  }

return true;
}

//----------------------------------------------------------------------------
void vtkVelodyneHDLPositionPacketInterpreter::ProcessPacket(unsigned char const * packetData,
                                                  unsigned int vtkNotUsed(dataLength))
{
  PositionPacket position;

  // Reinterpret the memory
  for (int i = 0; i < 3; ++i)
  {
    memcpy(position.gyro + i, packetData + 14 + i * 8, 2);
    memcpy(position.temp + i, packetData + 14 + i * 8 + 2, 2);
    memcpy(position.accelx + i, packetData + 14 + i * 8 + 4, 2);
    memcpy(position.accely + i, packetData + 14 + i * 8 + 6, 2);
  }

  for (int i = 0; i < 3; ++i)
  {
    // Selector only least significant 12 bits
    position.gyro[i] &= BIT_12_MASK;
    position.temp[i] &= BIT_12_MASK;
    position.accelx[i] &= BIT_12_MASK;
    position.accely[i] &= BIT_12_MASK;

    // Perform 12 bit twos complement
    position.gyro[i] =
      -2048 * ((position.gyro[i] & SIGN_12_MASK) >> 11) + (position.gyro[i] & REMAINDER_12_MASK);
    position.temp[i] =
      -2048 * ((position.temp[i] & SIGN_12_MASK) >> 11) + (position.temp[i] & REMAINDER_12_MASK);
    position.accelx[i] = -2048 * ((position.accelx[i] & SIGN_12_MASK) >> 11) +
      (position.accelx[i] & REMAINDER_12_MASK);
    position.accely[i] = -2048 * ((position.accely[i] & SIGN_12_MASK) >> 11) +
      (position.accely[i] & REMAINDER_12_MASK);
  }

  memcpy(&position.tohTimestamp, packetData + 14 + 3 * 8 + 160, 4);

  // ethernet payload starts at byte 2A, PPS byte is at F4 inside full ethernet
  // frame, so PPS in payload is at F4 - 2A = 244 - 42 = 202
  memcpy(&position.PPSSync, packetData + 202, 1);

  const int sentence_start =  14 + 8 + 8 + 8 + 160 + 4 + 4;
  std::copy(packetData + sentence_start,
            packetData + sentence_start + 306,
            position.sentance);
  // protection to terminate the string in case the sentence does not fit in the
  // 306 bytes.
  position.sentance[305] = '\0';

  if (!hasLastLidarUpdateTime)
  {
    hasLastLidarUpdateTime = true;
    lastLidarUpdateTime = position.tohTimestamp;
  }
  else
  {
    if (position.tohTimestamp - lastLidarUpdateTime < - 1e6 * 0.5 * 3600.0)
    {
      // tod wrap detected
      lidarTimeOffset += 1e6 * 3600.0;
    }
  }
  double convertedLidarUpdateTime = position.tohTimestamp + lidarTimeOffset;

  // Fill all arrays with information
  double x, y, z, lat, lon, heading, gpsUpdateTime;
  if (std::string(position.sentance).size() == 0)
  {
    // If there is no sentence to parse (no gps connected),
    // we use the following:
    x = 0.0;
    y = 0.0;
    z = 0.0;
    lat = 0.0;
    lon = 0.0;
    heading = 0.0;
    // the following value is wrong (no gps update so no update time),
    // so it should also be 0.0 (invalid)
    // but this value is kept to not risk breaking anything:
    gpsUpdateTime = position.tohTimestamp;
  }
  else
  {
    NMEAParser parser;
    NMEALocation parsedNMEA;
    parsedNMEA.Init();
    std::string NMEASentence = std::string(position.sentance);
    boost::trim_right(NMEASentence);
    std::vector<std::string> NMEAwords = parser.SplitWords(NMEASentence);
    if (!parser.ChecksumValid(NMEASentence))
    {
      vtkGenericWarningMacro("NMEA sentence: "
                             << "<" << NMEASentence << ">"
                             << "has invalid checksum");
      // TODO: should we skip or should we expect lazy NMEA implementers ?
    }

    if ((this->UseGPGGASentences && !parser.IsGPGGA(NMEAwords))
        || (!this->UseGPGGASentences && !parser.IsGPRMC(NMEAwords)))
    {
      return; // not the NMEA sentence we are interested in, skipping
    }

    if ( !( (parser.IsGPGGA(NMEAwords) && parser.ParseGPGGA(NMEAwords, parsedNMEA))
         || (parser.IsGPRMC(NMEAwords) && parser.ParseGPRMC(NMEAwords, parsedNMEA)) ))
    {
      vtkGenericWarningMacro("Failed to parse NMEA sentence: "
                             << "<" << NMEASentence << ">");
      return; // skipping this PositionPacket
    }

    // Gathering information on time synchronization between Lidar & GPS,
    // see Velodyne doc mentioned at definition of PositionPacket above.
    // We assume the Lidar will remain synchronized on gps (UTC) time even
    // if some NMEA packets without fix are received later (tunnel, hill ...).
    if (!this->PPSSynced
        && position.PPSSync == PPSState::PPS_LOCKED
        && parsedNMEA.Valid)
    {
      this->PPSSynced = true;
    }
    this->LastPPSState = static_cast<PPSState>(position.PPSSync);

    lat = parsedNMEA.Lat;
    lon = parsedNMEA.Long;
    x = 0.0;
    y = 0.0;
    this->utmProjector.Project(lat, lon, x, y);
    z = 0.0;
    // If sentence is GPGGA,  we have a chance to get an altitude
    if (parser.IsGPGGA(NMEAwords))
    {
      if (parsedNMEA.HasAltitude)
      {
        if (parsedNMEA.HasGeoidalSeparation)
        {
          // setting z to Height above ellipsoid
          // (coherent with setting 'datum=WGS84' in proj4)
          z = parsedNMEA.Altitude + parsedNMEA.GeoidalSeparation;
        }
        else
        {
          // Two possibilities: either Altitude is actually height above
          // ellipsoid, or it is effectively height above local MSL.
          // In this case we could need something better than this
          // (such as a look up table for geoid separation like GeographicLib)
          z = parsedNMEA.Altitude;
        }
      }
      else
      {
        z = 0.0;
      }
    }

    if (parsedNMEA.HasTrackAngle)
    {
      heading = parsedNMEA.TrackAngle;
    }
    else
    {
      heading = 0.0;
    }

    gpsUpdateTime = parsedNMEA.UTCSecondsOfDay;
    if (!hasLastGPSUpdateTime)
    {
      hasLastGPSUpdateTime = true;
      lastGPSUpdateTime = gpsUpdateTime;
    }
    else
    {
      if (gpsUpdateTime - lastGPSUpdateTime < - 12.0 * 3600.0)
      {
        // tod wrap detected
        GPSTimeOffset += 24.0 * 3600.0;
      }
    }

    convertedGPSUpdateTime = gpsUpdateTime + GPSTimeOffset;
    if (previousConvertedGPSUpdateTime < 0.0)
    {
      previousConvertedGPSUpdateTime = convertedGPSUpdateTime;
    }

    if (convertedGPSUpdateTime > previousConvertedGPSUpdateTime
        && parsedNMEA.Valid)
    {
      // We have detected that this position packet is the first one since
      // last gps fix (there are more position packets than there are fixes)
      // and that the new NMEA sentence refers to a valid fix,
      // so we can do an estimation of the timeshift.
      this->HasTimeshiftEstimation = true;
      // To understand this formula, think that we want to add this timeshift
      // to a lidar time to get a gps time, and that the "lidar instant" that
      // corresponds to the new fix is earlier than convertedLidarUpdateTime,
      // because of the time it took the information to go from GPS to Lidar.
      // Possible improvement: store the different estimations
      // (one per new fix) and return the median.
      this->TimeshiftMeasurements.push_back(
          convertedGPSUpdateTime -
          (1e-6 * convertedLidarUpdateTime - this->AssumedHardwareLag));
    }
    previousConvertedGPSUpdateTime = convertedGPSUpdateTime;
  }

  if (this->pointcount == 0)
  {
    this->Offset[0] = x;
    this->Offset[1] = y;
  }

  x -= this->Offset[0];
  y -= this->Offset[1];

  // Update the transforms here and then call internal transform
  if (SensorTransform) this->SensorTransform->Update();

  // Apply sensor transform
  double pos[3] = {x, y, z};
  if (SensorTransform) this->SensorTransform->InternalTransformPoint(pos, pos);

  this->points->InsertNextPoint(pos[0], pos[1], pos[2]);

  this->lats->InsertNextValue(lat);
  this->lons->InsertNextValue(lon);
  this->times->InsertNextValue(convertedLidarUpdateTime);
  this->gpsTime->InsertNextValue(convertedGPSUpdateTime);

  this->gyro1->InsertNextValue(position.gyro[0] * GYRO_SCALE);
  this->gyro2->InsertNextValue(position.gyro[1] * GYRO_SCALE);
  this->gyro3->InsertNextValue(position.gyro[2] * GYRO_SCALE);
  this->temp1->InsertNextValue(position.temp[0] * TEMP_SCALE + TEMP_OFFSET);
  this->temp2->InsertNextValue(position.temp[1] * TEMP_SCALE + TEMP_OFFSET);
  this->temp3->InsertNextValue(position.temp[2] * TEMP_SCALE + TEMP_OFFSET);
  this->accel1x->InsertNextValue(position.accelx[0] * ACCEL_SCALE);
  this->accel2x->InsertNextValue(position.accelx[1] * ACCEL_SCALE);
  this->accel3x->InsertNextValue(position.accelx[2] * ACCEL_SCALE);
  this->accel1y->InsertNextValue(position.accely[0] * ACCEL_SCALE);
  this->accel2y->InsertNextValue(position.accely[1] * ACCEL_SCALE);
  this->accel3y->InsertNextValue(position.accely[2] * ACCEL_SCALE);
  this->heading->InsertNextValue(heading);

  this->pointcount++;
}

void vtkVelodyneHDLPositionPacketInterpreter::FillInterpolatorFromPositionOrientation()
{
  // Optionally interpolate the GPS values... note that we assume that the
  // first GPS point is not 0,0 if we have valid GPS data; otherwise we assume
  // that the GPS data is garbage and ignore it
  if (lats->GetNumberOfTuples() && lons->GetNumberOfTuples() &&
    (lats->GetValue(0) != 0.0 || lons->GetValue(0) != 0.0))
  {
    this->FillInterpolatorFromGPSAndHeading(points, gpsTime, times, this->heading);
  }
}

//-----------------------------------------------------------------------------
vtkCustomTransformInterpolator* vtkVelodyneHDLPositionPacketInterpreter::GetInterpolator()
{
  return this->Interp.GetPointer();
}

//-----------------------------------------------------------------------------
double vtkVelodyneHDLPositionPacketInterpreter::GetTimeshiftEstimation() {
  if (this->TimeshiftMeasurements.size() == 0)
  {
    vtkGenericWarningMacro("Error : timeshift estimation asked"
                           " but no measurement available.")
    return 0.0;
  }
  return ComputeMedian(this->TimeshiftMeasurements);
}

//-----------------------------------------------------------------------------
std::string vtkVelodyneHDLPositionPacketInterpreter::GetTimeSyncInfo()
{
  std::string PPSDesc;
  if (this->LastPPSState == PPSState::PPS_ABSENT)
  {
    PPSDesc = "absent";
  }
  else if (this->LastPPSState == PPSState::PPS_ATTEMPTING_TO_SYNC)
  {
    PPSDesc = "attempting to sync";
  }
  else if (this->LastPPSState == PPSState::PPS_LOCKED)
  {
    PPSDesc = "locked";
  }
  else if (this->LastPPSState == PPSState::PPS_ERROR)
  {
    PPSDesc = "error";
  }
  else
  {
    PPSDesc = "unknown";
  }

  if  (!this->PPSSynced && this->HasTimeshiftEstimation)
  {
    std::ostringstream timeshiftEstimation;
    timeshiftEstimation << std::fixed << std::setprecision(6)
                        << this->GetTimeshiftEstimation();
    std::ostringstream assumedHWLag;
    assumedHWLag << std::fixed << std::setprecision(6)
                        << this->AssumedHardwareLag;
    vtkGenericWarningMacro("Recovered timeshift despite missing PPS sync: "
                        << timeshiftEstimation.str()
                        << " seconds (assuming hardware lag of: "
                        << assumedHWLag.str()
                        << " seconds)."
                        << " Add this value to the lidar timestamps (ToH)"
                        << " to get GPS UTC time (mod. 1 hour).");
  }

  return " Lidar PPS signal: "
      + PPSDesc
      + " - "
      + (this->PPSSynced ? "Lidar clock synced on GPS UTC ToH"
                         : "NO Lidar clock sync on GPS UTC ToH")
      + " ";
}
